﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;

namespace Microsoft.Samples.Kinect.BodyBasics.Controls
{
    public class Servo
    {
        public const int MAX_PWM = 2500;
        public const int MIN_PWM = 500;
        public const int MID_PWM = 1500;

        public delegate void PWMChangedHandler(int pwm);
        public event PWMChangedHandler PWMChangedEvent;

        //PWM阈值
        private int maxPwmRange = MAX_PWM;
        private int minPwmRange = MIN_PWM;

        //初始PWM值
        private int initPwm = 1500;

        //Angle阈值
        private int maxAngleRange;
        private int minAngleRange;

        private static int servoCounter = 0;

        private int number;
        private int abs_pwm;
        private int diff_pwm;
        private int pwm;

        public Servo(int number)
        {
            this.number = number;
            servoCounter++;
            abs_pwm = MID_PWM;
            diff_pwm = 0;
        }

        public Servo(int number,int absPWM,int diffPWM)
        {
            this.number = number;
            this.abs_pwm = absPWM;
            this.diff_pwm = diffPWM;
            servoCounter++;

        }
        //为了方便编号而创建的方法
        public Servo()
        {
            servoCounter++;
            this.number = servoCounter;
            this.abs_pwm = MID_PWM;
            this.diff_pwm = 0;
        }

        public int Abs_pwm
        {
            get
            {
                int result = initPwm + diff_pwm;
                if (result > MaxPwmRange)
                {
                    result = MaxPwmRange;
                }
                else if (result < MinPwmRange)
                {
                    result = MinPwmRange;
                }
                return result;
            }
            set
            {
                abs_pwm = value;
                diff_pwm = abs_pwm - initPwm;
                if (PWMChangedEvent != null)
                {
                    PWMChangedEvent(Abs_pwm);
                }
            }
        }

        public int Diff_pwm {
            get => diff_pwm;
            set
            {
                diff_pwm = value;
                abs_pwm = initPwm + diff_pwm;
                if (PWMChangedEvent != null)
                {
                    PWMChangedEvent(Abs_pwm);
                }
            }
        }
        public int Number { get => number; set => number = value; }
        /// <summary>
        /// 带有阀值的PWM值，滑动条的大小不代表确切的大小
        /// </summary>
        public int Pwm
        {
            get
            {
                int result = abs_pwm + diff_pwm;
                if (result > MaxPwmRange)
                {
                    result = MaxPwmRange;
                }
                else if (result < MinPwmRange)
                {
                    result = MinPwmRange;
                }
                return result;
            }
        }

        public int MaxPwmRange { get => maxPwmRange; set => maxPwmRange = value; }
        public int MinPwmRange { get => minPwmRange; set => minPwmRange = value; }
        public int InitPwm {
            get => initPwm;
            set
            {
                
                initPwm = value;
                Abs_pwm = initPwm;
            }
        }

        //舵机的角度和PWM波不一定对应，即常数是不一定的，或比例系数不定，导致麻烦
        //public int MaxAngleRange { get => maxAngleRange; set => maxAngleRange = value; }
        //public int MinAngleRange { get => minAngleRange; set => minAngleRange = value; }

        //private int angle2PWM(int angle)
        //{

        //}
        //private int PWM2angle(int pwm)
        //{

        //}

        public string GetEncoderString()
        {
            string toSend = "#";
            
            toSend += number.ToString();
            toSend += "P";
            //int pwm = abs_pwm + diff_pwm;
            toSend += Pwm.ToString();
            return toSend;
        }

        public static int deltaDegree2pwm(double degree,int range)
        {
            int pwm = 0;
            switch (range)
            {
                case 120: pwm = (int)(degree * 8.3);break;
                case 270: pwm = (int)(degree * 7.4);break;
                case 180: pwm = (int)(degree * 11);break;
                default:  pwm = (int)(degree * (2000 / range));break;
            }
            return pwm;

        }
    }
}
